%author: wxj233
%time: 2023.12.12 9:47
%function: 不敏卡尔曼滤波测试

clc;
clear;
clear Function;
close all;


% 初始化仿真器
% dt
simulater = Simulater(1);

% 产生第一个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster1 = simulater.generateFeaturePoints(10, 2.2, 1, 1);
% 根据特征点群，产生第一条轨迹
% cluster, point0, v, a, startTime, endTime, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
T = simulater.generateTrack_ra(cluster1, [10, 0], [0, 1], [0.01, -0.005], 0, 400, 1, 0.15, 0.1, 0.01, 10, 20);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
% polarscatter(T(:,3), T(:,2));
% data = load('T.mat');
% T = data.T;


dt = 1;
dr = 0.15*0.15;
dvr = 0.1*0.1;
dtheta = 0.01*0.01;

T1 = T(1, :);
T2 = T(2, :);
p1 = [T1(2)*cos(T1(3)), T1(2)*sin(T1(3))];
p2 = [T2(2)*cos(T2(3)), T2(2)*sin(T2(3))];
% 初始状态
Xk_e = [p2(1), (p2(1)-p1(1))/dt, p2(2), (p2(2)-p1(2))/dt]';

% 计算初始状态误差协方差
C1 = [cos(T1(3)), -T1(2)*sin(T1(3));
    sin(T1(3)), T1(2)*cos(T1(3))];
R1 = C1*[dr, 0; 0, dtheta]*C1';
C2 = [cos(T2(3)), -T2(2)*sin(T2(3));
    sin(T2(3)), T2(2)*cos(T2(3))];
R2 = C2*[dr, 0; 0, dtheta]*C2';
Pk_ee = [R2(1,1), R2(1,1)/dt, R2(1,2), R2(1,2)/dt;
         R2(1,1)/dt, (R2(1,1)+R1(1,1))/(dt*dt), R2(1,2)/dt, (R2(1,2)+R1(1,2))/(dt*dt);
         R2(2,1), R2(2,1)/dt, R2(2,2), R2(2,2)/dt;
         R2(2,1)/dt, (R2(2,1)+R1(2,1))/(dt*dt), R2(2,2)/dt, (R2(2,2)+R1(2,2))/(dt*dt)];

Q = 0.1 * eye(4);
R = [dr, 0, 0;
     0, dvr, 0;
     0, 0, dtheta];

A = [1, dt, 0, 0;
     0, 1, 0, 0;
     0, 0, 1, dt;
     0, 0, 0, 1];
ps = [];
ops = [];
for i = 3:1:size(T, 1)
    Xk_p = A*Xk_e;
    Pk_pe = A*Pk_ee*A' + Q;

%     [Zk_p, Pz, Pxz] = CuT(@h, Xk_p, Pk_pe);  % 量测预测、方差、以及和原状态的协方差,通过不敏变换计算
    [Zk_p, Pz, Pxz] = UT(@h, Xk_p, Pk_pe);  % 量测预测、方差、以及和原状态的协方差,通过不敏变换计算
%     disp(Pz);

    
    Zk = T(i, 2:4)';
    Vk = Zk - Zk_p;  % 新息（量测残差）
    Sk = Pz + R;  % 新息协方差,Pz就是量测预测的协方差，通过不敏变换计算可得

    Gk = Pxz/Sk;  % Pxz是Xk_pe和Zk_p之间的协方差矩阵,通过不敏变换计算得到

    Xk_e = Xk_p + Gk*Vk; 
    ps = [ps; Xk_e'];
    op = [T(i, 2)*sin(T(i, 3)), T(i, 2)*cos(T(i, 3))];
    ops = [ops; op];
    Pk_ee = Pk_pe - Gk*Sk*Gk';
    disp(Pk_ee);
end
figure(1);
plot(ps(:,1), ps(:,3));
hold on;
gscatter(ops(:,2), ops(:,1));



function Zk = h(Xk_e)
  % 非线性变换函数
  % Xk_e: 原状态[x, vx, y, vy]'
  x = Xk_e(1,1);
  vx = Xk_e(2,1);
  y = Xk_e(3,1);
  vy = Xk_e(4,1);

  r = sqrt(x*x+y*y);
  theta = atan2(y, x);
  vr = [x, y]*[vx, vy]'/norm([x, y]);

  Zk = [r, theta, vr]';
end


function [Zk_p, Pz, Pxz] = UT(h, Xk_p, Pk_pe)
  % 不敏变换
  % h: 非线性变换关系
  % Xk_p: 原状态期望值[x, vx, y, vy]'
  % Pk_pe: 原状态误差协方差矩阵
  alpha = 0.001;  % 通常情况取一小正值
  beta = 2;  % 高斯情况下beta最优值为2
  tau = 0; % tau通常取0
  Zdim = 3;

  N = size(Xk_p, 1);
  X=[];  % 样本序列
  Wm = [];  % 一阶权重
  Wc = [];  % 二阶权重
  Z = [];  % 每一个样本对应线性变换后的值
  Zk_p = zeros([Zdim, 1]);
  Pz = zeros([Zdim, Zdim]);
  Pxz = zeros([N, Zdim]);

  lambda = alpha^2*(N+tau) - N;
  P=(chol((N+lambda)*Pk_pe))';  % 取列的话这个地方就要转置
  for n=0:1:(2*N)
      if n == 0
          X = [X, Xk_p];
          Wm = [Wm, lambda/(lambda+N)];
          Wc = [Wc, lambda/(lambda+N)+1-alpha^2+beta];
      else
          if n<=N
              X = [X, Xk_p+P(:, n)];
          else
              X = [X, Xk_p-P(:, n-N)];
          end

          Wm = [Wm, 0.5/(lambda+N)];
          Wc = [Wc, 0.5/(lambda+N)];
      end

%       disp(X(:,n+1));
      Z = [Z, h(X(:,n+1))];

      Zk_p = Zk_p + Wm(n+1)*Z(:, n+1);
  end

  for n=0:1:N*2
      Pz = Pz + Wc(n+1)*(Z(:, n+1) - Zk_p)*(Z(:, n+1) - Zk_p)';
      Pxz = Pxz + Wc(n+1)*(X(:, n+1) - Xk_p)*(Z(:, n+1) - Zk_p)';
  end
end


function [Zk_p, Pz, Pxz] = CuT(h, Xk_p, Pk_pe)
  % 容积变换
  % h: 非线性变换关系
  % Xk_p: 原状态期望值[x, vx, y, vy]'
  % Pk_pe: 原状态误差协方差矩阵

  N = size(Xk_p, 1);
  Zdim = 3;

  X = Xk_p + chol(Pk_pe)'*[eye(N), -eye(N)];  % 容积点（样本点s）
  W = 0.5/N;  % 权重
  
  Z = [];
  Zk_p = zeros([Zdim, 1]);
  Pz = zeros([Zdim, Zdim]);
  Pxz = zeros([N, Zdim]);
  for n = 1:1:2*N
      Z = [Z, h(X(:,n))];
      Zk_p = Zk_p + W*Z(:, n);

      Pz = Pz + W*Z(:, n)*Z(:, n)';
      Pxz = Pxz + W*X(:,n)*Z(:, n)';
  end

  Pz = Pz - Zk_p*Zk_p';
  Pxz = Pxz - Xk_p*Zk_p';
end












